#include "rs485.h"
#include <errno.h>
#include <fcntl.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <termios.h>
#include <unistd.h>

// 全局RS485文件描述符
static int g_rs485_fd = -1;

int configure_serial(int fd, int baudrate) {
  struct termios options;
  if (tcgetattr(fd, &options) != 0) {
    perror("tcgetattr");
    return -1;
  }

  speed_t speed;
  switch (baudrate) {
  case 9600:
    speed = B9600;
    break;
  case 19200:
    speed = B19200;
    break;
  case 38400:
    speed = B38400;
    break;
  case 57600:
    speed = B57600;
    break;
  case 115200:
    speed = B115200;
    break;
  case 921600:
    speed = B921600;
    break;
  default:
    speed = B9600;
    break;
  }
  cfsetispeed(&options, speed);
  cfsetospeed(&options, speed);

  options.c_cflag &= ~PARENB;
  options.c_cflag &= ~CSTOPB;
  options.c_cflag &= ~CSIZE;
  options.c_cflag |= CS8;
  options.c_cflag |= CREAD | CLOCAL;

  options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
  options.c_iflag &= ~(IXON | IXOFF | IXANY);
  options.c_iflag &= ~(INLCR | IGNCR | ICRNL);
  options.c_oflag &= ~OPOST;

  options.c_cc[VTIME] = 10;
  options.c_cc[VMIN] = 0;

  if (tcsetattr(fd, TCSANOW, &options) != 0) {
    perror("tcsetattr");
    return -1;
  }
  return 0;
}

int rs485_init(const char *device, int baudrate) {
  int fd = open(device, O_RDWR | O_NOCTTY);
  if (fd < 0) {
    perror("Failed to open RS485 device");
    return -1;
  }

  if (configure_serial(fd, baudrate) < 0) {
    close(fd);
    return -1;
  }

  return fd;
}

int rs485_read_optical_encoder(int fd, uint32_t *encoder_value) {
  if (fd < 0 || encoder_value == NULL) {
    return -1;
  }

  // 光栅尺读取命令：FF 03 00 01 00 02 80 15
  uint8_t command[] = {0xFF, 0x03, 0x00, 0x01, 0x00, 0x02, 0x80, 0x15};
  uint8_t response[64];
  int bytes_written, bytes_read;

  // 清空接收缓冲区
  tcflush(fd, TCIFLUSH);

  // 发送命令
  bytes_written = write(fd, command, sizeof(command));
  if (bytes_written != sizeof(command)) {
    printf("RS485: Write failed, expected %zu bytes, actual %d bytes\n",
           sizeof(command), bytes_written);
    return -1;
  }

  // 等待响应
  usleep(50000); // 50ms延时

  // 读取响应
  bytes_read = read(fd, response, sizeof(response));
  if (bytes_read <= 0) {
    printf("RS485: No response received from optical encoder\n");
    return -1;
  }

  // 验证响应格式
  if (bytes_read < 7) {
    printf("RS485: Invalid response length: %d\n", bytes_read);
    return -1;
  }

  // 检查响应格式：地址码、功能码、数据长度
  if (response[0] != 0xFF || response[1] != 0x03 || response[2] != 0x04) {
    printf("RS485: Invalid response format\n");
    return -1;
  }

  // 解析32位数据 (根据实际协议：前两字节为高16位，后两字节为低16位)
  uint16_t high16 = (response[3] << 8) | response[4]; // 0x709D
  uint16_t low16 = (response[5] << 8) | response[6];  // 0x0624
  *encoder_value = ((uint32_t)low16 << 16) | high16; // 正确组合：0x0624709D

  return 0;
}

// 初始化光栅尺通信
int grating_scale_init(void) {
  if (g_rs485_fd != -1) {
    return 0; // 已经初始化
  }

  g_rs485_fd = rs485_init("/dev/ttyS2", 9600);
  if (g_rs485_fd < 0) {
    printf("光栅尺RS485初始化失败\n");
    return -1;
  }

  printf("光栅尺RS485初始化成功\n");
  return 0;
}

// 读取光栅尺当前值
int grating_scale_read_current_value(uint32_t *current_value) {
  if (g_rs485_fd < 0) {
    printf("RS485未初始化\n");
    return -1;
  }

  return rs485_read_optical_encoder(g_rs485_fd, current_value);
}